#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav_msg_pkg/msg/waypoint.hpp"
#include "nav_msg_pkg/msg/nav_feedback.hpp"

using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
using Waypoint = nav_msg_pkg::msg::Waypoint;
class NavServer : public rclcpp::Node
{
public:
    NavServer() : Node("nav_server")
    {
        RCLCPP_INFO(this->get_logger(), "nav_server started");
        // 创建导航客户端
        navigate_to_pose_client_ = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");
        // 订阅导航目标点
        navigation_sub_ = this->create_subscription<Waypoint>(
            "/nav_server/nav_point", 10, std::bind(&NavServer::nav_point_callback, this, std::placeholders::_1));
        // 导航进度发布
        feedback_pub_ = this->create_publisher<nav_msg_pkg::msg::NavFeedback>("/nav_server/nav_feedback", 10);
        // 导航结果
        result_pub_ = this->create_publisher<std_msgs::msg::String>("/nav_server/nav_result", 10);
        // 订阅导航取消
        navigation_cancel_sub_ = this->create_subscription<std_msgs::msg::String>(
            "/nav_server/nav_cancel", 10, std::bind(&NavServer::nav_cancel_callback, this, std::placeholders::_1));
        RCLCPP_INFO(this->get_logger(), "nav_server 已启动.");
        // 等待导航服务启动
        while (!navigate_to_pose_client_->wait_for_action_server(std::chrono::seconds(1)))
        {
            // 对 ctrl + c 退出程序
            if (!rclcpp::ok())
            {
                RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "退出！");
                return;
            }
            RCLCPP_INFO(this->get_logger(), "Waiting for the navigate_to_pose action server...");
        }
    }

private:
    rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_to_pose_client_;
    rclcpp::Subscription<Waypoint>::SharedPtr navigation_sub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr result_pub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr navigation_cancel_sub_;
    rclcpp::Publisher<nav_msg_pkg::msg::NavFeedback>::SharedPtr feedback_pub_;
    Waypoint point_;

    // 接收到航点
    void nav_point_callback(const Waypoint::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "接收任务,目标航点 name=%s,type=%s,x=%f,y=%f",
                    msg->name.c_str(),
                    msg->type.c_str(),
                    msg->pose.position.x,
                    msg->pose.position.y);
        point_ = *msg;
        send_goal(point_);
    }

    void goal_response_callback(GoalHandleNavigateToPose::SharedPtr goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
        }
    }

    void feedback_callback(GoalHandleNavigateToPose::SharedPtr, const std::shared_ptr<const NavigateToPose::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "Distance remaining: %.3f m", feedback->distance_remaining);
        nav_msg_pkg::msg::NavFeedback feedback_msg_;
        feedback_msg_.distance_remaining = feedback->distance_remaining;
        feedback_msg_.current_pose = feedback->current_pose;
        feedback_pub_->publish(feedback_msg_);
    }

    void result_callback(const GoalHandleNavigateToPose::WrappedResult &result)
    {
        std_msgs::msg::String result_msg_;
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "导航 Goal succeeded!");
            result_msg_.data = "SUCCEEDED";
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "导航 Goal was aborted");
            result_msg_.data = "ABORTED";
            break;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "导航 Goal was canceled");
            result_msg_.data = "CANCELED";
            break;
        default:
            RCLCPP_ERROR(this->get_logger(), "导航 Unknown result code");
            result_msg_.data = "UNKNOWN";
            break;
        }
        result_pub_->publish(result_msg_);
    }

    void send_goal(const Waypoint &point)
    {
        RCLCPP_INFO(this->get_logger(), "开始导航，目标点 name=%s,type=%s,x=%f,y=%f",
                    point.name.c_str(),
                    point.type.c_str(),
                    point.pose.position.x,
                    point.pose.position.y);

        auto goal_msg = NavigateToPose::Goal();
        goal_msg.pose.header.frame_id = "map";
        goal_msg.pose.header.stamp = this->get_clock()->now();
        goal_msg.pose.pose = point.pose;

        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();

        send_goal_options.goal_response_callback = std::bind(&NavServer::goal_response_callback, this, std::placeholders::_1);

        // send_goal_options.feedback_callback = std::bind(&NavServer::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback = std::bind(&NavServer::result_callback, this, std::placeholders::_1);

        navigate_to_pose_client_->async_send_goal(goal_msg, send_goal_options);
    }
    /**
     * 导航取消
     */
    void nav_cancel_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "收到导航取消指令，取消导航");
        navigate_to_pose_client_->async_cancel_all_goals(); // 取消所有任务
        // navigate_to_pose_client_->async_cancel_goal(point_.name);//取消指定任务
        // navigate_to_pose_client_->async_cancel_goals_before();//取消在当前时间点之前的所有任务
    }
};
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<NavServer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}